// This file is part of the MatrixPilot LedTest demo.
//
//    http://code.google.com/p/gentlenav/
//
// Copyright 2009-2011 MatrixPilot Team
// See the AUTHORS.TXT file for a list of authors of MatrixPilot.
//
// MatrixPilot is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MatrixPilot is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MatrixPilot.  If not, see <http://www.gnu.org/licenses/>.


#include "../../libUDB/libUDB.h"
#include "../../libUDB/mpu6000.h"
#include "../../libUDB/servoOut.h"
#include "../../libUDB/ADchannel.h"
#include "../../libUDB/heartbeat.h"
#include "../../libUDB/eeprom_udb4.h"
#include <libpic30.h>

#if (BOARD_TYPE == UDB4_BOARD)
#define RATE_THRESHOLD_LED      120
#define ACCEL_THRESHOLD_LED     40
#else
#define RATE_THRESHOLD_LED      200
#define ACCEL_THRESHOLD_LED     120
#endif

int x_rate,  y_rate,  z_rate;
int x_accel, y_accel, z_accel;
char calib_countdown = 10;
char eepromFailureFlashCount = 32;
boolean eepromSuccess = 0;

extern void IOTest(void);
void udb_background_callback_triggered(void);


int main(void)
{
	mcu_init();
	IOTest();
	udb_init();
#if (BOARD_TYPE != AUAV3_BOARD) // NOTE: this is already done in udb_init() above
	udb_eeprom_init();  // using legacy eeprom driver
#endif
	DPRINT("MatrixPilot LedTest\r\n");
	while (1)
	{
		udb_run();
	}
	return 0;
}

// Called every 1/40 second at low priority
void udb_heartbeat_40hz_callback(void)
{
	if (udb_heartbeat_counter % 20 != 0) return;

	switch (calib_countdown) {
#if (BOARD_TYPE == UDB4_BOARD || BOARD_TYPE == UDB5_BOARD)
		case 9:
			udb_background_trigger(&udb_background_callback_triggered);
			led_on(LED_RED);
			break;
		case 8:
			led_off(LED_RED);
			led_on(LED_GREEN);
			break;
		case 7:
			led_off(LED_GREEN);
			led_on(LED_ORANGE);
			break;
		case 6:
			led_off(LED_ORANGE);
			led_on(LED_BLUE);
			break;
		case 5:
			led_off(LED_BLUE);
			led_on(LED_RED);
			break;
		case 4:
			led_off(LED_RED);
			led_on(LED_GREEN);
			break;
		case 3:
			led_off(LED_GREEN);
			led_on(LED_ORANGE);
			break;
		case 2:
			led_on(LED_BLUE);
			led_off(LED_ORANGE);
			break;
		case 1:
			led_off(LED_BLUE);
			udb_a2d_record_offsets();
			break;
#elif (BOARD_TYPE == AUAV3_BOARD)
		case 9:
			udb_background_trigger(&udb_background_callback_triggered);
			led_on(LED_BLUE);
			break;
		case 8:
			led_off(LED_BLUE);
			led_on(LED_RED);
			break;
		case 7:
			led_off(LED_RED);
			led_on(LED_GREEN);
			break;
		case 6:
			led_off(LED_GREEN);
			led_on(LED_ORANGE);
			break;
		case 5:
			led_off(LED_ORANGE);
			led_on(LED_BLUE);
			break;
		case 4:
			led_off(LED_BLUE);
			led_on(LED_RED);
			break;
		case 3:
			led_off(LED_RED);
			led_on(LED_GREEN);
			break;
		case 2:
			led_off(LED_GREEN);
			led_on(LED_ORANGE);
			break;
		case 1:
			led_off(LED_ORANGE);
			udb_a2d_record_offsets();
			break;
#elif (BOARD_TYPE == PX4_BOARD)
#else
#error "unsupported BOARD_TYPE"
#endif
		case 0: // after 5 sec
			return;
	}
	calib_countdown--;
}

void udb_background_callback_triggered(void)
{
#if (BOARD_TYPE != AUAV3_BOARD)
	// Write 1,2,3,4 into the first 4 bytes of the EEPROM
	unsigned char data[4] = {1, 2, 3, 4};
	eeprom_PageWrite(0x0000, data, 4);

	// Read the first 4 bytes of the EEPROM
	data[0] = data[1] = data[2] = data[3] = 0;
	eeprom_SequentialRead(0x0000, data, 4);

	// Then test them
	if (data[0] == 1 && data[1] == 2 && data[2] == 3 && data[3] == 4) {
		eepromSuccess = 1;
	}
#endif
}

// Called at 40 Hz, before reseting the sensor sampling
void udb_callback_read_sensors(void)
{
	x_rate = XRATE_VALUE;
	y_rate = YRATE_VALUE;
	z_rate = ZRATE_VALUE;

	x_accel = XACCEL_VALUE;
	y_accel = YACCEL_VALUE;
	z_accel = ZACCEL_VALUE;
}

// Called at HEARTBEAT_HZ, before sending servo pulses
void udb_heartbeat_callback(void)
{
	if (calib_countdown) {
		udb_pwOut[ROLL_OUTPUT_CHANNEL]    = 3000;
		udb_pwOut[PITCH_OUTPUT_CHANNEL]   = 3000;
		udb_pwOut[YAW_OUTPUT_CHANNEL]     = 3000;
		udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] = 3000;
		udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] = 3000;
		udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] = 3000;
	} else if (eepromSuccess == 0 && eepromFailureFlashCount) {
		// eeprom failure!
		DPRINT("eeprom failure!\r\n");
		if (udb_heartbeat_counter % 6 == 0) {
			udb_led_toggle(LED_RED);
			udb_led_toggle(LED_GREEN);
			udb_led_toggle(LED_BLUE);
			udb_led_toggle(LED_ORANGE);
			eepromFailureFlashCount--;
		}
	} else {
		union longww accum;

		accum.WW = __builtin_mulss(y_rate, 4000);
		udb_pwOut[ROLL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

		accum.WW = __builtin_mulss(x_rate, 4000);
		udb_pwOut[PITCH_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

		accum.WW = __builtin_mulss(z_rate, 4000);
		udb_pwOut[YAW_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

		accum.WW = __builtin_mulss(x_accel, 4000);
		udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

		accum.WW = __builtin_mulss(y_accel, 4000);
		udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

		accum.WW = __builtin_mulss(z_accel, 4000);
		udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);

		if ((udb_heartbeat_counter / 600) % 2 == 0) {
			led_on(LED_RED);
			((abs(udb_pwOut[ROLL_OUTPUT_CHANNEL]  - 3000) > RATE_THRESHOLD_LED) ? led_on(LED_ORANGE) : led_off(LED_ORANGE));
			((abs(udb_pwOut[PITCH_OUTPUT_CHANNEL] - 3000) > RATE_THRESHOLD_LED) ? led_on(LED_BLUE) : led_off(LED_BLUE));
			((abs(udb_pwOut[YAW_OUTPUT_CHANNEL]   - 3000) > RATE_THRESHOLD_LED) ? led_on(LED_GREEN) : led_off(LED_GREEN));
		} else {
			led_off(LED_RED);
			((abs(udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? led_on(LED_ORANGE) : led_off(LED_ORANGE));
			((abs(udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? led_on(LED_BLUE) : led_off(LED_BLUE));
			((abs(udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? led_on(LED_GREEN) : led_off(LED_GREEN));
		}
	}
}

//int16_t udb_gps_callback_get_byte_to_send(void) { return -1; }
//void udb_gps_callback_received_byte(uint8_t rxchar) { }
//int16_t udb_serial_callback_get_byte_to_send(void) { return -1; }
//void udb_serial_callback_received_byte(uint8_t rxchar) { }

void udb_callback_radio_did_turn_off() { }
//void init_events(void) { }
//void osd_init(void) { }
